﻿using System;
using System.Collections.Generic;
using System.Text;
using dnAnalytics.LinearAlgebra;
using VectorMath;
using dnAnalytics.LinearAlgebra.Solvers.Iterative;
using dnAnalytics.LinearAlgebra.Solvers.Preconditioners;
using dnAnalytics.LinearAlgebra.Solvers;

namespace FluidSystem
{
    public class FluidSystem
    {

        /// <summary>
        /// NEVER CHANGE OR USE THIS!!! ASSUMPTIONS HAVE BEEN MADE.
        /// </summary>
        private const int H = 1;
        /// <summary>
        /// NEVER CHANGE OR USE THIS!!! ASSUMPTIONS HAVE BEEN MADE.
        /// </summary>
        private const int h = 1;


        //these three define our world size. each cell is 1.0 x 1.0 x 1.0.
        //we access them from 1 to grid____ (MATLAB style) for things that actually exist
        //indices 0 and grid___ + 1 store the boundaries.
        public int GridHeight;
        public int GridWidth;
        public int GridDepth;

        float GravityForce = -10;
        float ThermalDiffusion = 0.5f; //should be < 1.0 ?
        public double maxViscosity = 1000, minViscosity = 0.001, solidTransition = 95, liquidTransition = 105;
        float initialGlobalTemp = 10f;
        float density = 1.0f;

        float k_cfl = 0.5f;
        //should be set the same at the dt argument by the calculaton code
        public float dt_cfl
        {
            get;
            private set;
        }

        public enum State
        {
            EMPTY, SURFACE, FULL
        };

        public BoundedGrid<State> CellsStates
        {
            get;
            private set;
        }

        public ICollection<Particle> Particles
        {
            get;
            set;
        }

        private BoundedGrid<float> P_Temp;
        //pressure data structure is for debugging only, not an actual state
        public BoundedGrid<float> Pressure
        {
            get;
            private set;
        }

        private BoundedGrid<Pnt3D> V_Temp;

        public BoundedGrid<Pnt3D> Velocities
        {
            get;
            private set;
        }

        private BoundedGrid<float> T_Temp;

        public BoundedGrid<float> Temperatures
        {
            get;
            private set;
        }

        /// <summary>
        /// set boundary velocities so that there is no flow in/out from bounds
        /// Neuman-BC like the smoke starter code.
        /// Tangential equals inside, which is slip. Change for friction-like effect.
        /// </summary>
        private void setBoundaryNV()
        {


            /*
            //Z direction faces
            for (int i = 1; i <= GridWidth; i++) {
                for (int e = 1; e <= GridHeight; e++) {
                    //normal
                    V_Temp[i, e, 1].z = 0;
                    V_Temp[i, e, GridDepth + 1].z = 0;
                    //tangential
                    V_Temp[i, e, 1].x = V_Temp[i, e, 1].x;
                    V_Temp[i, e, 1].y = V_Temp[i, e, 1].y;
                    V_Temp[i, e, GridDepth + 1].x = V_Temp[i, e, GridDepth].x;
                    V_Temp[i, e, GridDepth + 1].y = V_Temp[i, e, GridDepth].y;
                }
            }

            //Y direction faces
            for (int i = 1; i <= GridWidth; i++) {
                for (int o = 1; o <= GridDepth; o++) {
                    //normal
                    V_Temp[i, 1, o].y = 0;
                    V_Temp[i, GridHeight + 1, o].y = 0;
                    //tangential
                    V_Temp[i, 1, o].x = V_Temp[i, 1, o].x;
                    V_Temp[i, 1, o].z = V_Temp[i, 1, o].z;
                    V_Temp[i, GridHeight + 1, o].x = V_Temp[i, GridHeight, o].x;
                    V_Temp[i, GridHeight + 1, o].z = V_Temp[i, GridHeight, o].z;
                }
            }

            //X direction faces
            for (int o = 1; o <= GridDepth; o++) {
                for (int e = 1; e <= GridHeight; e++) {
                    //normal
                    V_Temp[1, e, o].x = 0;
                    V_Temp[GridWidth + 1, e, o].x = 0;
                    //tangential
                    V_Temp[1, e, o].y = V_Temp[1, e, o].y;
                    V_Temp[1, e, o].z = V_Temp[1, e, o].z;
                    V_Temp[GridWidth + 1, e, o].y = V_Temp[GridDepth, e, o].y;
                    V_Temp[GridWidth + 1, e, o].z = V_Temp[GridWidth, e, o].z;
                }
            }
            */
        } //CURRENTLY NOT USED

        Dictionary<Pnt3D, Pnt3D> traces, velocityTraces;

        public FluidSystem(int x, int y, int z)
        {
            GridWidth = x;
            GridHeight = y;
            GridDepth = z;

            Velocities = new BoundedVelocityGrid(x, y, z);
            V_Temp = new BoundedVelocityGrid(x, y, z);

            //    Velocities.SetBorders(new Pnt3D(0, 0, 0)); //friction borders for now
            //    V_Temp.SetBorders(new Pnt3D(0, 0, 0));

            Temperatures = new BoundedGrid<float>(x, y, z);
            T_Temp = new BoundedGrid<float>(x, y, z);

            Pressure = new BoundedGrid<float>(x, y, z);
            P_Temp = new BoundedGrid<float>(x, y, z);


            Particles = new List<Particle>();

            CellsStates = new BoundedGrid<State>(x, y, z);

            traces = new Dictionary<Pnt3D, Pnt3D>(x * y * z);
            velocityTraces = new Dictionary<Pnt3D, Pnt3D>(x * y * z);

            for (int i = 0; i < x; i++)
                for (int j = 0; j < y; j++)
                    for (int k = 0; k < z; k++)
                    {
                        Velocities[i, j, k] = new Pnt3D(0, 0, 0);
                        V_Temp[i, j, k] = new Pnt3D(0, 0, 0);
                        Temperatures[i, j, k] = initialGlobalTemp;
                    }

            timeStepCalculations(0.00001f); //take a tiny time step to initialize the system
        }

        //use to insert flame
        public void setCellTemp(float i, float e, float o, double temperature)
        {
            Temperatures[(int)(i), (int)(e), (int)(o)] = (float)temperature;
        }

        void advection(float dt)
        {
            for (int i = 0; i < GridWidth; i++)
                for (int e = 0; e < GridHeight; e++)
                    for (int o = 0; o < GridDepth; o++)
                    {
                        Pnt3D xOriginal = traceParticle(new Pnt3D(i, e + 0.5, o + 0.5), -dt, Velocities);
                        Pnt3D yOriginal = traceParticle(new Pnt3D(i + 0.5, e, o + 0.5), -dt, Velocities);
                        Pnt3D zOriginal = traceParticle(new Pnt3D(i + 0.5, e + 0.5, o), -dt, Velocities);

                        V_Temp[i, e, o].x = getVelocity(xOriginal, Velocities).x;
                        V_Temp[i, e, o].y = getVelocity(yOriginal, Velocities).y;
                        V_Temp[i, e, o].z = getVelocity(zOriginal, Velocities).z;
                    }

        }



        public BoundedGrid<double> getViscosity()
        {
            BoundedGrid<double> rv = new BoundedGrid<double>(GridWidth, GridHeight, GridDepth);
            rv.SetBorders(0);
            for (int i = 0; i < GridWidth; i++)
                for (int j = 0; j < GridHeight; j++)
                    for (int k = 0; k < GridDepth; k++)
                    {
                        if (CellsStates[i, j, k] == State.EMPTY) {
                            rv[i, j, k] = 0;
                        } else if (Temperatures[i, j, k] > liquidTransition) {
                            rv[i, j, k] = minViscosity;
                        } else if (Temperatures[i, j, k] < solidTransition) {
                            rv[i, j, k] = maxViscosity;
                        } else {
                            rv[i, j, k] = maxViscosity - (Temperatures[i, j, k] - solidTransition) * (maxViscosity - minViscosity) / (liquidTransition - solidTransition);
                        }
                    }
            return rv;
        }

        double geoMean(double d1, double d2)
        {
            double rv =  Math.Exp((Math.Log(d1) + Math.Log(d2)) / 2);
            return (Double.IsNaN(rv)) ? 0.0 : rv;
        }

        double geoMean(double d1, double d2, double d3, double d4) {
            double rv = Math.Exp((Math.Log(d1) + Math.Log(d2) + Math.Log(d3) + Math.Log(d4))/4);
            return (Double.IsNaN(rv)) ? 0.0 : rv;
        }

        /// <summary>
        /// also called viscosity
        /// </summary>
        void diffusion(float dt)
        {

            //pull out free-falling blob bulk motion
            Dictionary<Pnt3I, List<Pnt3I>> blobs = getFreeFallingBlobs();

            // ==== calculat bulk motion of each blob ====
            Dictionary<Pnt3I, Pnt3D> blobVelocities = new Dictionary<Pnt3I, Pnt3D>();
            foreach (Pnt3I index in blobs.Keys)
            {
                //get avg velocity of this blob
                Pnt3D velocity = new Pnt3D(0, 0, 0);
                foreach (Pnt3I cell in blobs[index])
                {
                    Pnt3D V = getVelocity(new Pnt3D(cell.x, cell.y, cell.z), Velocities);
                    velocity.Add(new Pnt3D(V.x, V.y, V.z));
                }
                velocity.Times(1.0 / blobs[index].Count);
                blobVelocities.Add(index, velocity);
            }

            // ==== diffuse Velocities into V_Temp ====
            diffuse(dt);

            // ==== restore bulk blob motion ====

            //first calculate new bulk motion of each blob
            Dictionary<Pnt3I, Pnt3D> blobNewVelocities = new Dictionary<Pnt3I, Pnt3D>();
            foreach (Pnt3I index in blobs.Keys)
            {
                //get avg velocity of this blob
                Pnt3D velocity = new Pnt3D(0, 0, 0);
                foreach (Pnt3I cell in blobs[index])
                {
                    Pnt3D V = getVelocity(new Pnt3D(cell.x, cell.y, cell.z), V_Temp);
                    velocity.Add(new Pnt3D(V.x, V.y, V.z));
                }
                velocity.Times(1.0 / blobs[index].Count);
                blobNewVelocities.Add(index, velocity);
            }

            //calcuate differences
            Dictionary<Pnt3I, Pnt3D> blobVelocitiesDiff = new Dictionary<Pnt3I, Pnt3D>();
            foreach (Pnt3I index in blobs.Keys)
            {
                blobVelocitiesDiff.Add(index, Pnt3D.Difference(blobVelocities[index], blobNewVelocities[index]));
            }

            //add differences back in //add originals back in so it actually works
            foreach (Pnt3I index in blobs.Keys)
            {
                Pnt3D velocityFix = blobVelocities[index];
                //fix each face of each pt in this blob
                foreach (Pnt3I cell in blobs[index])
                {
                    velocityFix.Times(0.5);
                    V_Temp[cell.x, cell.y, cell.z].Add(velocityFix);
                    V_Temp[cell.x + 1, cell.y, cell.z].x += velocityFix.x;
                    V_Temp[cell.x, cell.y + 1, cell.z].y += velocityFix.y;
                    V_Temp[cell.x, cell.y, cell.z + 1].z += velocityFix.z;
                }
            }

        } //end of diffusion

        /// <summary>
        /// flatten an index
        /// </summary>
        int flatten(int i, int j, int k)
        {
            return k * (GridHeight) * (GridWidth) + j * (GridWidth) + i;
        }

        /// <summary>
        /// un-flatten an index, taking off any offsets
        /// </summary>
        Pnt3I flattenInv(int idx)
        {
            int i = idx % (GridWidth);
            int j = (idx % ((GridHeight) * (GridWidth))) / (GridWidth);
            int k = idx / ((GridHeight) * (GridWidth));

            return new Pnt3I(i, j, k);
        }


        /// <summary>
        /// flatten an index accounting for border
        /// </summary>
        int flattenWithBorder(int i, int j, int k)
        {
            return (k + 1) * (GridHeight + 2) * (GridWidth + 2) + (j + 1) * (GridWidth + 2) + (i + 1);
        }


        /// <summary>
        /// diffuse Velocities into V_Temp
        /// </summary>
        /// <param name="dt"></param>
        private void diffuse(float dt)
        {
            int gridSize = (GridWidth + 3) * (GridHeight + 3) * (GridDepth + 3);
            BoundedGrid<double> viscocities = getViscosity();
            Vector solutionVector;

            for (int i = 0; i < GridWidth; i++)
                for (int j = 0; j < GridHeight; j++)
                    for (int k = 0; k < GridDepth; k++)
                        V_Temp[i, j, k] = Velocities[i, j, k]; //initialize V_Temp since it is sparsely updated

            Vector uOld = new SparseVector(gridSize);
            Matrix D = new SparseMatrix(gridSize, gridSize);
            for (int i = 0; i <= GridWidth; i++)
                for (int j = 0; j < GridHeight; j++)
                    for (int k = 0; k < GridDepth; k++)
                    {
                      

                        //put in if this cell has fluid         //other cases lead to non-symmetric falls and don't work right
                        bool doit = CellsStates[i, j, k] != State.EMPTY || CellsStates[i - 1, j, k] != State.EMPTY;

                        if (doit)
                        {
                            int row = flattenWithBorder(i, j, k);
                            D[row, row] = 1;
                            uOld[row] = Velocities[i, j, k].x;

                            bool immobile = (i == 0 || i == GridWidth);
                            double v1 = viscocities[i - 1, j, k];
                            double v2 = geoMean(viscocities[i - 1, j, k], viscocities[i, j, k], viscocities[i - 1, j - 1, k], viscocities[i, j - 1, k]);
                            double v3 = geoMean(viscocities[i - 1, j, k], viscocities[i, j, k], viscocities[i - 1, j, k - 1], viscocities[i, j, k - 1]);
                            double v4 = geoMean(viscocities[i - 1, j, k], viscocities[i, j, k], viscocities[i - 1, j + 1, k], viscocities[i, j + 1, k]);
                            double v5 = geoMean(viscocities[i - 1, j, k], viscocities[i, j, k], viscocities[i - 1, j, k + 1], viscocities[i, j, k + 1]);
                            double v6 = viscocities[i, j, k];
                            //only put in matrix if this is not a face on the surface (neither cell is empty)
                            D[row, row] = immobile ? 1 : 1 + dt * (v1 + v2 + v3 + v4 + v5 + v6);
                            D[row, flattenWithBorder(i - 1, j, k)] = immobile ? 0 : -dt * v1;
                            D[row, flattenWithBorder(i, j - 1, k)] = immobile ? 0 : -dt * v2;
                            D[row, flattenWithBorder(i, j, k - 1)] = immobile ? 0 : -dt * v3;
                            D[row, flattenWithBorder(i, j + 1, k)] = immobile ? 0 : -dt * v4;
                            D[row, flattenWithBorder(i, j, k + 1)] = immobile ? 0 : -dt * v5;
                            D[row, flattenWithBorder(i + 1, j, k)] = immobile ? 0 : -dt * v6;
                        }
                    }

            solutionVector = Solve(uOld, D);

            for (int i = 0; i <= GridWidth; i++)
                for (int j = 0; j < GridHeight; j++)
                    for (int k = 0; k < GridDepth; k++) {
                        //put in uOld and maybe matrix if at least one cell is not empty
                        if ((CellsStates[i - 1, j, k] != State.EMPTY || CellsStates[i, j, k] != State.EMPTY)) {
                            if (Velocities[i, j, k].x == 0 && Double.IsNaN(solutionVector[flattenWithBorder(i, j, k)])) {
                                continue;
                            }
                            if (Double.IsNaN(solutionVector[flattenWithBorder(i, j, k)])) {
                                int dummy = 0;
                            }
                            V_Temp[i, j, k].x = solutionVector[flattenWithBorder(i, j, k)];
                        }
                    }




            uOld = new SparseVector(gridSize);
            D = new SparseMatrix(gridSize, gridSize);
            for (int i = 0; i < GridWidth; i++)
                for (int j = 0; j < GridHeight; j++)
                    for (int k = 0; k <= GridDepth; k++) {


                        //put in if this cell has fluid         //other cases lead to non-symmetric falls and don't work right
                        bool doit = CellsStates[i, j, k] != State.EMPTY || CellsStates[i, j, k-1] != State.EMPTY;

                        if (doit) {
                            int row = flattenWithBorder(i, j, k);
                            D[row, row] = 1;
                            uOld[row] = Velocities[i, j, k].z;

                            bool immobile = (k == 0 || k == GridDepth);
                            double v1 = viscocities[i, j, k-1];
                            double v2 = geoMean(viscocities[i - 1, j, k], viscocities[i, j, k], viscocities[i - 1, j, k-1], viscocities[i, j, k-1]);
                            double v3 = geoMean(viscocities[i, j-1, k], viscocities[i, j, k], viscocities[i, j-1, k - 1], viscocities[i, j, k - 1]);
                            double v4 = geoMean(viscocities[i + 1, j, k], viscocities[i, j, k], viscocities[i + 1, j, k-1], viscocities[i, j, k-1]);
                            double v5 = geoMean(viscocities[i, j+1, k], viscocities[i, j, k], viscocities[i, j+1, k - 1], viscocities[i, j, k - 1]);
                            double v6 = viscocities[i, j, k];
                            //only put in matrix if this is not a face on the surface (neither cell is empty)
                            D[row, row] = immobile ? 1 : 1 + dt * (v1 + v2 + v3 + v4 + v5 + v6);
                            D[row, flattenWithBorder(i, j, k - 1)] = immobile ? 0 : -dt * v1;
                            D[row, flattenWithBorder(i - 1, j, k)] = immobile ? 0 : -dt * v2;
                            D[row, flattenWithBorder(i, j - 1, k)] = immobile ? 0 : -dt * v3;
                            D[row, flattenWithBorder(i + 1, j, k)] = immobile ? 0 : -dt * v4;
                            D[row, flattenWithBorder(i, j + 1, k)] = immobile ? 0 : -dt * v5;
                            D[row, flattenWithBorder(i, j, k + 1)] = immobile ? 0 : -dt * v6;
                        }
                    }

            solutionVector = Solve(uOld, D);

            for (int i = 0; i < GridWidth; i++)
                for (int j = 0; j < GridHeight; j++)
                    for (int k = 0; k <= GridDepth; k++) {
                        //put in uOld and maybe matrix if at least one cell is not empty
                        if ((CellsStates[i, j, k - 1] != State.EMPTY || CellsStates[i, j, k] != State.EMPTY)) {
                            if (Velocities[i, j, k].z == 0 && Double.IsNaN(solutionVector[flattenWithBorder(i, j, k)])) {
                                continue;
                            }
                            if (Double.IsNaN(solutionVector[flattenWithBorder(i, j, k)])) {
                                int dummy = 0;
                            }
                             V_Temp[i, j, k].z = solutionVector[flattenWithBorder(i, j, k)];
                        }
                    }


            uOld = new SparseVector(gridSize);
            D = new SparseMatrix(gridSize, gridSize);
            for (int i = 0; i < GridWidth; i++)
                for (int j = 0; j <= GridHeight; j++)
                    for (int k = 0; k < GridDepth; k++) {


                        //put in if this cell has fluid         //other cases lead to non-symmetric falls and don't work right
                        bool doit = CellsStates[i, j, k] != State.EMPTY || CellsStates[i, j - 1, k] != State.EMPTY;

                        if (doit) {
                            int row = flattenWithBorder(i, j, k);
                            D[row, row] = 1;
                            uOld[row] = Velocities[i, j, k].y;


                            double v1 = viscocities[i, j - 1, k];
                            double v2 = geoMean(viscocities[i - 1, j, k], viscocities[i, j, k], viscocities[i - 1, j - 1, k], viscocities[i, j - 1, k]);
                            double v3 = geoMean(viscocities[i, j - 1, k], viscocities[i, j, k], viscocities[i, j - 1, k - 1], viscocities[i, j, k - 1]);
                            double v4 = geoMean(viscocities[i + 1, j, k], viscocities[i, j, k], viscocities[i + 1, j - 1, k], viscocities[i, j - 1, k]);
                            double v5 = geoMean(viscocities[i, j - 1, k], viscocities[i, j, k], viscocities[i, j - 1, k + 1], viscocities[i, j, k + 1]);
                            double v6 = viscocities[i, j, k];
                            //only put in matrix if this is not a face on the surface (neither cell is empty)
                            bool immobile = (j == 0 || j == GridHeight);
                            D[row, row] = immobile ? 1 : 1 + dt * (v1 + v2 + v3 + v4 + v5 + v6);
                            D[row, flattenWithBorder(i, j - 1, k)] = immobile ? 0 : -dt * v1;
                            D[row, flattenWithBorder(i - 1, j, k)] = immobile ? 0 : -dt * v2;
                            D[row, flattenWithBorder(i, j, k - 1)] = immobile ? 0 : -dt * v3;
                            D[row, flattenWithBorder(i + 1, j, k)] = immobile ? 0 : -dt * v4;
                            D[row, flattenWithBorder(i, j, k + 1)] = immobile ? 0 : -dt * v5;
                            D[row, flattenWithBorder(i, j + 1, k)] = immobile ? 0 : -dt * v6;
                        }
                    }

            solutionVector = Solve(uOld, D);

            for (int i = 0; i < GridWidth; i++)
                for (int j = 0; j <= GridHeight; j++)
                    for (int k = 0; k < GridDepth; k++) {
                        //put in uOld and maybe matrix if at least one cell is not empty
                        if ((CellsStates[i, j - 1, k] != State.EMPTY || CellsStates[i, j, k] != State.EMPTY)) {
                            if (Velocities[i, j, k].y == 0 && Double.IsNaN(solutionVector[flattenWithBorder(i, j, k)])) {
                                continue;
                            }
                            if (Double.IsNaN(solutionVector[flattenWithBorder(i, j, k)])) {
                                int dummy = 0;
                            }
                            V_Temp[i, j, k].y = solutionVector[flattenWithBorder(i, j, k)];
                        }
                    }



           // Vector uNewX, uNewY, uNewZ;

            //implicit approach, solve D * u_new = u_old;
            //uNewX = SolveOurs(uOldx, D);
            //uNewY = SolveOurs(uOldy, D);
            //uNewZ = SolveOurs(uOldz, D);

            //explicit approach, (-1*D + diag(2)) *u_old = u_new;
            //Matrix explicitD = ((D * -1) + new DiagonalMatrix(D.Rows, D.Columns, 2));
            //uNewX = explicitD * uOldx;
            //uNewY = explicitD * uOldy;
            //uNewZ = explicitD * uOldz;

      /*      for (int i = 0; i < GridWidth; i++)
                for (int j = 0; j < GridHeight; j++)
                    for (int k = 0; k < GridDepth; k++)
                    {
                        //grab result if we put the arguments in for it, otherwise the soln will not be meaningful
                        //V_Temp[i, j, k].x = uOldx[flattenWithBorder(i, j, k)] == 0 ? V_Temp[i, j, k].x : uNewX[flattenWithBorder(i, j, k)];
                        //V_Temp[i, j, k].y = uOldy[flattenWithBorder(i, j, k)] == 0 ? V_Temp[i, j, k].y : uNewY[flattenWithBorder(i, j, k)];
                        //V_Temp[i, j, k].z = uOldz[flattenWithBorder(i, j, k)] == 0 ? V_Temp[i, j, k].z : uNewZ[flattenWithBorder(i, j, k)];

                        //hack in pure viscosity values
                        V_Temp[i, j, k].x /= viscocities[i, j, k];
                        V_Temp[i, j, k].y /= viscocities[i, j, k];
                        V_Temp[i, j, k].z /= viscocities[i, j, k];
                    }  */
        }

        void extrapolateExterior()
        {

            for (int i = 0; i < GridWidth; i++)
                for (int j = 0; j < GridHeight; j++)
                    for (int k = 0; k < GridDepth; k++)
                        V_Temp[i, j, k] = Velocities[i, j, k]; //initialize V_Temp since it is sparsely updated

            //============== COMPLICATED IDEA, [Foster 96] in 3D ================
            // i+   i-  j+  j-  k+  k- are the 6 faces/neighbors, enumerate unique cases, then rotate/mirror to get all 64 cases
            // 0    0   0   0   0   0 - Surrounded by liquid, not a boundary
            // in general balance with opposide side, then get diffusion equal zero via centered difference
            // 0    0   0   0   0   1  k- = k+ - ((depth of cell / height)(j+ - j-) + (depth / width)(i+ - i-))
            // 0    0   0   0   1   1  k+ = k- = -((depth of cell / height)(j+ - j-)/2 + (depth / width)(i+ - i-)/2)
            // 0    0   0   1   0   1  
            // 0    0   0   1   1   1
            // 0    0   1   1   1   1
            // 0    1   0   1   0   1
            // 0    1   0   1   1   1
            // 0    1   1   1   1   1
            // 1    1   1   1   1   1  Surrounded by air, no diffusion



            //============== BETTER IDEA [for the rest of us] ================
            BoundedGrid<int> layerOfCell = new BoundedGrid<int>(GridWidth, GridHeight, GridDepth);
            layerOfCell.SetBorders(-1);

            //initialize layers
            for (int i = 0; i < GridWidth; i++)
                for (int j = 0; j < GridHeight; j++)
                    for (int k = 0; k < GridDepth; k++)
                    {
                        layerOfCell[i, j, k] = (CellsStates[i, j, k] == State.EMPTY) ? -1 : 0;
                    }

            for (int iter = 1; iter < Math.Max(2, Math.Ceiling(k_cfl)); iter++)
            {
                for (int i = 0; i < GridWidth; i++)
                    for (int j = 0; j < GridHeight; j++)
                        for (int k = 0; k < GridDepth; k++)
                        {
                            //for each cell such that cell layer is -1
                            if (layerOfCell[i, j, k] == -1)
                            {
                                //if cell has an adjacent fluid (or "recently updated surface" cell
                                bool right = layerOfCell[i + 1, j, k] == (iter - 1);
                                bool left = layerOfCell[i - 1, j, k] == (iter - 1);
                                bool up = layerOfCell[i, j + 1, k] == (iter - 1);
                                bool down = layerOfCell[i, j - 1, k] == (iter - 1);
                                bool front = layerOfCell[i, j, k - 1] == (iter - 1);
                                bool back = layerOfCell[i, j, k + 1] == (iter - 1);

                                //if cell has a neighbor with layer == iter - 1
                                if (right || left || up || down || front || back)
                                {
                                    //loop over faces
                                    for (int face = 0; face < 6; face++)
                                    {
                                        int iOff = (face == 0) ? -1 : ((face == 1) ? 1 : 0);
                                        int jOff = (face == 2) ? -1 : ((face == 3) ? 1 : 0);
                                        int kOff = (face == 4) ? -1 : ((face == 5) ? 1 : 0);
                                        double sum = 0.0;
                                        int count = 0;
                                        //if cell's neighbor at this face is an empty cell
                                        //for velocity components not bording fluid cells
                                        //set velocity to the avg of neighbors of C in which layer == iter -1
                                        if (CellsStates[i + iOff, j + jOff, k + kOff] == State.EMPTY)
                                        {
                                            //loop over neighbors of cell
                                            for (int face2 = 0; face2 < 6; face2++)
                                            {
                                                int iOff2 = (face2 == 0) ? -1 : ((face2 == 1) ? 1 : 0);
                                                int jOff2 = (face2 == 2) ? -1 : ((face2 == 3) ? 1 : 0);
                                                int kOff2 = (face2 == 4) ? -1 : ((face2 == 5) ? 1 : 0);
                                                if (layerOfCell[i + iOff2, j + jOff2, k + kOff2] == iter - 1)
                                                {
                                                    if (face < 2)
                                                        sum += V_Temp[i + iOff2 + face, j + jOff2, k + kOff2].x;
                                                    else if (face < 4)
                                                    {
                                                        sum += V_Temp[i + iOff2, j + jOff2 - 2 + face, k + kOff2].y;
                                                    }
                                                    else
                                                        sum += V_Temp[i + iOff2, j + jOff2, k + kOff2 - 4 + face].z;
                                                    count++;
                                                }
                                            }
                                            double nv = sum / count;
                                            int iOff3 = (face == 0) ? 0 : ((face == 1) ? 1 : 0);
                                            int jOff3 = (face == 2) ? 0 : ((face == 3) ? 1 : 0);
                                            int kOff3 = (face == 4) ? 0 : ((face == 5) ? 1 : 0);
                                            if (face < 2)
                                            {
                                                V_Temp[i + iOff3, j + jOff3, k + kOff3].x = nv;
                                            }
                                            else if (face < 4)
                                            {
                                                V_Temp[i + iOff3, j + jOff3, k + kOff3].y = nv;
                                            }
                                            else
                                            {
                                                V_Temp[i + iOff3, j + jOff3, k + kOff3].z = nv;
                                            }

                                        }
                                    }

                                    layerOfCell[i, j, k] = iter;
                                }

                            }
                        }
            }

        }

        /// <summary>
        /// collect external forces into velocities for this timestep
        /// </summary>
        void body(float dt)
        {
            for (int i = 0; i < GridWidth; i++)
                for (int j = 0; j < GridHeight; j++)
                    for (int k = 0; k < GridDepth; k++)
                    {
                        if (CellsStates[i, j - 1, k] != State.EMPTY || CellsStates[i, j, k] != State.EMPTY)
                        {
                            V_Temp[i, j, k].y += GravityForce * dt;
                        }
                    }
        }

        /// <summary>
        /// heat equations, woot!
        /// </summary>
        void temperature(float dt)
        {
            BoundedGrid<float> ttemp;

            //advect temperatures with particles
            for (int i = 0; i < GridWidth; i++)
                for (int j = 0; j < GridHeight; j++)
                    for (int k = 0; k < GridDepth; k++)
                    {
                        //initialize
                        T_Temp[i, j, k] = Temperatures[i, j, k];

                        if (CellsStates[i, j, k] == State.EMPTY) continue;
                        Pnt3D x0 = new Pnt3D(i, j, k);
                        Pnt3D xOriginal = traceParticle(x0, -dt, Velocities);
                        T_Temp[i, j, k] = getTemperature(xOriginal);

                    }


            //swap new temperatures into old data structure
            ttemp = Temperatures;
            Temperatures = T_Temp;
            T_Temp = ttemp;

            for (int i = 0; i < GridWidth; i++)
                for (int j = 0; j < GridHeight; j++)
                    for (int k = 0; k < GridDepth; k++)
                    {
                        // q_new = q + k_therm * dt * laplacian(q) // coded as q_new = laplacian(q) * k_therm * dt + q
                        T_Temp[i, j, k] = -6 * Temperatures[i, j, k]
                            + Temperatures[i + 1, j, k] + Temperatures[i - 1, j, k]
                            + Temperatures[i, j + 1, k] + Temperatures[i, j - 1, k]
                            + Temperatures[i, j, k + 1] + Temperatures[i, j, k - 1];
                        T_Temp[i, j, k] *= ThermalDiffusion * dt;
                        T_Temp[i, j, k] += Temperatures[i, j, k];
                    }

            //swap new temperatures into kld data structure
            ttemp = Temperatures;
            Temperatures = T_Temp;
            T_Temp = ttemp;

        }

        /// <summary>
        /// step 3d of [for the rest of us]
        /// poisson equation
        /// diffusion free
        /// </summary>
        private void calculatePressure(float dt)
        {
            int gridSize = (GridWidth) * (GridHeight) * (GridDepth);

            Vector B = new SparseVector(gridSize);
            Matrix A = new SparseMatrix(gridSize, gridSize);

            for (int i = 0; i < GridWidth; i++)
                for (int j = 0; j < GridHeight; j++)
                    for (int k = 0; k < GridDepth; k++)
                    {
                        if (CellsStates[i, j, k] != State.EMPTY)
                        {
                            //build A

                            int count = 0;
                            //loop over faces, set this cell's neighbor spots
                            for (int x = 0; x < 6; x++)
                            {
                                int iOff = (x == 0) ? -1 : ((x == 1) ? 1 : 0);
                                int jOff = (x == 2) ? -1 : ((x == 3) ? 1 : 0);
                                int kOff = (x == 4) ? -1 : ((x == 5) ? 1 : 0);
                                //if cell's neighbor at this face is an empty cell
                                if (CellsStates[i + iOff, j + jOff, k + kOff] == State.EMPTY && !isBoundary(i + iOff, j + jOff, k + kOff))
                                {
                                    count++;
                                }
                                else if (CellsStates[i + iOff, j + jOff, k + kOff] != State.EMPTY && !isBoundary(i + iOff, j + jOff, k + kOff))
                                {
                                    count++;
                                    A[flatten(i, j, k), flatten(i + iOff, j + jOff, k + kOff)] = 1;
                                }
                            }
                            //now set this cell's self spot
                            A[flatten(i, j, k), flatten(i, j, k)] = -count;

                            //build B
                            float divU = (float)((Velocities[i + 1, j, k].x - Velocities[i, j, k].x) +
                                        (Velocities[i, j + 1, k].y - Velocities[i, j, k].y) +
                                        (Velocities[i, j, k + 1].z - Velocities[i, j, k].z));
                            B[flatten(i, j, k)] = density / dt * divU;
                        }
                    }

            Vector P = SolveOurs(B, A);

            for (int i = 0; i < P.Count; i++)
            {
                Pnt3I indx = flattenInv(i);
                if (CellsStates[indx.x, indx.y, indx.z] != State.EMPTY)
                {
                    P_Temp[indx.x, indx.y, indx.z] = (float)P[i];
                }
                else
                {
                    if (!isBoundary(indx.x, indx.y, indx.z))
                        P_Temp[indx.x, indx.y, indx.z] = 0.0f;
                }
            }
            Pressure = P_Temp;

            //set pressure at boundaries? maybe? (likely okay w/o)
        }

        private bool isBoundary(int x, int y, int z)
        {
            return (x == -1 || x == GridWidth || y == -1 || y == GridHeight || z == -1 || z == GridDepth);
        }

        /// <summary>
        /// write to V_Temp from Velocity and gradient of Pressure
        /// </summary>
        private void applyPressure(float dt)
        {
            for (int i = 0; i < GridWidth; i++)
                for (int j = 0; j < GridHeight; j++)
                    for (int k = 0; k < GridDepth; k++)
                    {
                        //V_Temp[i,j,k] = Velocities[i,j,k] - dt/density * gradP; //impossible

                        //backwards difference approach
                        Pnt3D gradP = new Pnt3D(Pressure[i, j, k] - Pressure[i - 1, j, k],
                                    Pressure[i, j, k] - Pressure[i, j - 1, k],
                                    Pressure[i, j, k] - Pressure[i, j, k - 1]);
                        gradP.Times(dt / density);

                        V_Temp[i, j, k].x = Velocities[i, j, k].x - gradP.x;
                        V_Temp[i, j, k].y = Velocities[i, j, k].y - gradP.y;
                        V_Temp[i, j, k].z = Velocities[i, j, k].z - gradP.z;
                    }
        }

        public void updateCellStates(float dt)
        {

            BoundedGrid<State> newStates = new BoundedGrid<State>(GridWidth, GridHeight, GridDepth);
            newStates.SetBorders(State.EMPTY);
            for (int i = 0; i < GridWidth; i++)
                for (int j = 0; j < GridHeight; j++)
                    for (int k = 0; k < GridDepth; k++)
                    {
                        newStates[i, j, k] = State.EMPTY;
                    }

            foreach (Particle p in Particles)
            {
                newStates[(int)(p.X.x), (int)(p.X.y), (int)(p.X.z)] = State.FULL;
            }

            //borders stored as empty, although it doesn't matter in the rest of code
            //set X surfaces
            for (int i = 0; i < GridWidth; i++)
                for (int j = 0; j < GridHeight; j++)
                    for (int k = 0; k < GridDepth; k++)
                    {
                        if (newStates[i, j, k] == State.FULL)
                        {
                            if (newStates[i - 1, j, k] == State.EMPTY || newStates[i + 1, j, k] == State.EMPTY ||
                                newStates[i, j - 1, k] == State.EMPTY || newStates[i, j + 1, k] == State.EMPTY ||
                                newStates[i, j, k - 1] == State.EMPTY || newStates[i, j, k + 1] == State.EMPTY)
                                newStates[i, j, k] = State.SURFACE;
                        }
                    }

            CellsStates = newStates;
        }

        /// <summary>
        /// snap X0 inside bounds
        /// you should only need this after moving a point via velocity
        /// </summary>
        /// <param name="X0"></param>
        private void snapToBounds(Pnt3D X0)
        {
            if (X0.x < 0)
                X0.x = 0;
            if (X0.x >= GridWidth - 0.0001)
                X0.x = GridWidth - 0.0001;
            if (X0.y < 0)
                X0.y = 0;
            if (X0.y >= GridHeight - 0.0001)
                X0.y = GridHeight - 0.0001;
            if (X0.z < 0)
                X0.z = 0;
            if (X0.z >= GridDepth - 0.0001)
                X0.z = GridDepth - 0.0001;
        }

        //Trace a particle from Pnt3D (x, y, z) for t time using RK2 (midpoint). [for the rest of us]
        Pnt3D traceParticle(Pnt3D X0, double t, BoundedGrid<Pnt3D> velocities)
        {
            if (t < 0 && traces.ContainsKey(X0)) //t < 0 since all backtraces the same, gives 33% speedup
                return traces[X0];

            Pnt3D V = getVelocity(X0, velocities);
            Pnt3D M = new Pnt3D(X0.x + 0.5 * t * V.x, X0.y + 0.5 * t * V.y, X0.z + 0.5 * t * V.z);
            snapToBounds(M);
            V = getVelocity(M, velocities);
            V.Times(t);

            Pnt3D X1 = new Pnt3D(X0.x, X0.y, X0.z);
            X1.Add(V);
            snapToBounds(X1);

            traces[X0] = X1; //store this result for later

            return X1;
        }
        //Get the interpolated velocity at a Pnt3D in space. [for the rest of us]
        public Pnt3D getVelocity(Pnt3D p, BoundedGrid<Pnt3D> velocities)
        {
            if (velocityTraces.ContainsKey(p)) //since all traces the same, gives another 33% speedup
                return velocityTraces[p];

            velocityTraces[p] = getinterpolatedValue(p.x, p.y, p.z, velocities); //store it for later
            return velocityTraces[p];
        }
        //Getaninterpolateddatavaluefromthegrid. [for the rest of us]
        Pnt3D getinterpolatedValue(double x, double y, double z, BoundedGrid<Pnt3D> velocities)
        {
            Pnt3D V = new Pnt3D(0, 0, 0);

            int i = (int)Math.Floor(x);
            int j = (int)Math.Floor(y - 0.5);
            int k = (int)Math.Floor(z - 0.5);

            double i0 = 1 - (x - i);
            double j0 = 1 - (y - (j + 0.5));
            double k0 = 1 - (z - (k + 0.5));

            V.x = (i0) * (j0) * (k0) * (velocities[i, j, k].x) +
            (1 - i0) * (j0) * (k0) * (velocities[i + 1, j, k].x) +
            (i0) * (1 - j0) * (k0) * (velocities[i, j + 1, k].x) +
            (1 - i0) * (1 - j0) * (k0) * (velocities[i + 1, j + 1, k].x) +
            (i0) * (j0) * (1 - k0) * (velocities[i, j, k + 1].x) +
            (1 - i0) * (j0) * (1 - k0) * (velocities[i + 1, j, k + 1].x) +
            (i0) * (1 - j0) * (1 - k0) * (velocities[i, j + 1, k + 1].x) +
            (1 - i0) * (1 - j0) * (1 - k0) * (velocities[i + 1, j + 1, k + 1].x);

            i = (int)Math.Floor(x - 0.5);
            j = (int)Math.Floor(y);
            k = (int)Math.Floor(z - 0.5);


            i0 = 1 - (x - (i + 0.5));
            j0 = 1 - (y - j);
            k0 = 1 - (z - (k + 0.5));

            V.y = (i0) * (j0) * (k0) * (velocities[i, j, k].y) +
            (1 - i0) * (j0) * (k0) * (velocities[i + 1, j, k].y) +
            (i0) * (1 - j0) * (k0) * (velocities[i, j + 1, k].y) +
            (1 - i0) * (1 - j0) * (k0) * (velocities[i + 1, j + 1, k].y) +
            (i0) * (j0) * (1 - k0) * (velocities[i, j, k + 1].y) +
            (1 - i0) * (j0) * (1 - k0) * (velocities[i + 1, j, k + 1].y) +
            (i0) * (1 - j0) * (1 - k0) * (velocities[i, j + 1, k + 1].y) +
            (1 - i0) * (1 - j0) * (1 - k0) * (velocities[i + 1, j + 1, k + 1].y);

            i = (int)Math.Floor(x - 0.5);
            j = (int)Math.Floor(y - 0.5);
            k = (int)Math.Floor(z);

            i0 = 1 - (x - (i + 0.5));
            j0 = 1 - (y - (j + 0.5));
            k0 = 1 - (z - k);

            V.z = (i0) * (j0) * (k0) * (velocities[i, j, k].z) +
            (1 - i0) * (j0) * (k0) * (velocities[i + 1, j, k].z) +
            (i0) * (1 - j0) * (k0) * (velocities[i, j + 1, k].z) +
            (1 - i0) * (1 - j0) * (k0) * (velocities[i + 1, j + 1, k].z) +
            (i0) * (j0) * (1 - k0) * (velocities[i, j, k + 1].z) +
            (1 - i0) * (j0) * (1 - k0) * (velocities[i + 1, j, k + 1].z) +
            (i0) * (1 - j0) * (1 - k0) * (velocities[i, j + 1, k + 1].z) +
            (1 - i0) * (1 - j0) * (1 - k0) * (velocities[i + 1, j + 1, k + 1].z);

            return V;
        }

        float getTemperature(Pnt3D p)
        {
            int i = (int)Math.Floor(p.x);
            int j = (int)Math.Floor(p.y);
            int k = (int)Math.Floor(p.z);

            return (float)((i + 1 - p.x) * (j + 1 - p.y) * (k + 1 - p.z) * (Temperatures[i, j, k]) +
                            (p.x - i) * (j + 1 - p.y) * (k + 1 - p.z) * (Temperatures[i + 1, j, k]) +
                            (i + 1 - p.x) * (p.y - j) * (k + 1 - p.z) * (Temperatures[i, j + 1, k]) +
                            (p.x - i) * (p.y - j) * (k + 1 - p.z) * (Temperatures[i + 1, j + 1, k]) +
                            (i + 1 - p.x) * (j + 1 - p.y) * (p.z - k) * (Temperatures[i, j, k + 1]) +
                            (p.x - i) * (j + 1 - p.y) * (p.z - k) * (Temperatures[i + 1, j, k + 1]) +
                            (i + 1 - p.x) * (p.y - j) * (p.z - k) * (Temperatures[i, j + 1, k + 1]) +
                            (p.x - i) * (p.y - j) * (p.z - k) * (Temperatures[i + 1, j + 1, k + 1]));

        }

        public void moveParticles(float dt)
        {
            foreach (Particle p in Particles)
            {
                p.X = traceParticle(p.X, dt, Velocities);
            }
        }

        public Dictionary<Pnt3I, List<Pnt3I>> getFreeFallingBlobs()
        {
            FluidSet fluidSet = new FluidSet();

            for (int i = 0; i < GridWidth; i++)
                for (int j = 0; j < GridHeight; j++)
                    for (int k = 0; k < GridDepth; k++)
                    {
                        if (CellsStates[i, j, k] != State.EMPTY)
                        {
                            bool floating = (j > 1); //above ground
                            fluidSet.indicesToCells.Add(new Pnt3I(i, j, k), new FluidSetElement(floating, i, j, k));
                        }
                    }

            for (int i = 0; i < GridWidth; i++)
                for (int j = 0; j < GridHeight; j++)
                    for (int k = 0; k < GridDepth; k++)
                    {
                        fluidSet.Union(new Pnt3I(i, j, k), new Pnt3I(i + 1, j, k));
                        fluidSet.Union(new Pnt3I(i, j, k), new Pnt3I(i, j + 1, k));
                        fluidSet.Union(new Pnt3I(i, j, k), new Pnt3I(i, j, k + 1));
                    }

            Dictionary<Pnt3I, List<Pnt3I>> blobs = new Dictionary<Pnt3I, List<Pnt3I>>();
            foreach (FluidSetElement fse in fluidSet.indicesToCells.Values)
            {
                FluidSetElement fseFound = fluidSet.Find(fse.CellID);
                if (fseFound.IsFloating)
                {
                    //check if already in dictionary
                    if (blobs.ContainsKey(fseFound.CellID))
                    {
                        //get that list out and add this elem
                        blobs[fseFound.CellID].Add(fse.CellID);
                    }
                    else
                    {
                        //create list then add this elem
                        blobs.Add(fseFound.CellID, new List<Pnt3I>());
                        blobs[fseFound.CellID].Add(fse.CellID);
                    }
                }
            }

            return blobs;
        }

        bool timing = false;

        int counter;

        /// <summary>
        /// take a time step of size dt
        /// </summary>
        /// <param name="dt"></param>
        /// <returns>size of time step used internally (for debug views)</returns>
        public void timeStep(float dt)
        {
            if (--counter < 0)
            {
                timeStepCalculations(dt);
                counter = 0;// (int)(dt_cfl / dt); //how many movements of dt we can do in this dt_cfl
                Console.Out.WriteLine("using dt_cfl = {0:G4} seconds", dt_cfl);
            }

            //4.Move the particles through u for ?t time.
            //  4a.If ?t extends beyond the next displayed frame time
            //      Advance particles to the next displayed frame.
            //      Display frame and repeat step4a.
            //  4b.Move particles through u for the remainder of ?t.
            sw1.Reset();
            sw1.Start();
            moveParticles(dt_cfl);
            sw1.Stop();
            if (timing) Console.Out.WriteLine("move particles took {0:G4} seconds", sw1.ElapsedMilliseconds / 1000.0f);
        }

        private float timeStepCalculations(float dt)
        {
            traces.Clear(); //buffer for backtraces.
            velocityTraces.Clear();

            //ordering of these based off "Fluid Flow for the Rest of Us"

            // 1.Calculate the simulation time step, ?t .
            double maxV = 0;
            for (int i = 0; i < GridWidth; i++)
                for (int j = 0; j < GridHeight; j++)
                    for (int k = 0; k < GridDepth; k++)
                        maxV = Math.Max(maxV, getVelocity(new Pnt3D(i, j, k), Velocities).Length);

            //if (Math.Abs(maxV) > double.Epsilon && !double.IsNaN(maxV) && !double.IsInfinity(maxV)) { dt = Math.Min((float)(k_cfl / maxV), 0.1f); }

            dt_cfl = dt;

            BoundedGrid<Pnt3D> temp; //extra pointer for swapping

            //2.Update the grid based on the marker particles.
            sw1.Reset();
            sw1.Start();
            updateCellStates(dt);
            sw1.Stop();
            if (timing) Console.Out.WriteLine("cell states took {0:G4} seconds", sw1.ElapsedMilliseconds / 1000.0f);

            //  2b. advect temperature with backwards particle trace.
            //      diffuse temperature
            sw1.Reset();
            sw1.Start();
            temperature(dt);
            sw1.Stop();
            if (timing) Console.Out.WriteLine("temperature took {0:G4} seconds", sw1.ElapsedMilliseconds / 1000.0f);

            //3.Advance the velocity field, u.

            //  3a.Apply convection using a backwards particle trace.
            sw1.Reset();
            sw1.Start();
            advection(dt);
            sw1.Stop();
            if (timing) Console.Out.WriteLine("advection took {0:G4} seconds", sw1.ElapsedMilliseconds / 1000.0f);

            temp = Velocities;
            Velocities = V_Temp;
            V_Temp = temp;

            //  3b.Apply external forces.
            body(dt);

            temp = Velocities;
            Velocities = V_Temp;
            V_Temp = temp;

            //  3c.Apply viscosity. Use geometric mean to avg between empty and non-empty cells.
            sw1.Reset();
            sw1.Start();
            diffusion(dt);
            sw1.Stop();
            if (timing) Console.Out.WriteLine("diffusion took {0:G4} seconds", sw1.ElapsedMilliseconds / 1000.0f);

            temp = Velocities;
            Velocities = V_Temp;
            V_Temp = temp;


            //  3d.Calculate the pressure to satisfy del·u = 0.
            sw1.Reset();
            sw1.Start();
            calculatePressure(dt);
            sw1.Stop();
            if (timing) Console.Out.WriteLine("pressure took {0:G4} seconds", sw1.ElapsedMilliseconds / 1000.0f);

            //  3e.Apply the pressure.
            sw1.Reset();
            sw1.Start();
            applyPressure(dt);
            sw1.Stop();
            if (timing) Console.Out.WriteLine("applying pressure took {0:G4} seconds", sw1.ElapsedMilliseconds / 1000.0f);

            temp = Velocities;
            Velocities = V_Temp;
            V_Temp = temp;


            //  3f.Extrapolate fluid velocities into buffer zone
            sw1.Reset();
            sw1.Start();
            extrapolateExterior();
            sw1.Stop();
            if (timing) Console.Out.WriteLine("extrapolate Exterior took {0:G4} seconds", sw1.ElapsedMilliseconds / 1000.0f);

            temp = Velocities;
            Velocities = V_Temp;
            V_Temp = temp;

            return dt;
        }

        System.Diagnostics.Stopwatch sw1 = new System.Diagnostics.Stopwatch();


        /// <summary>
        /// solve Ax = b, return x.
        /// </summary>
        private static Vector Solve(Vector b, Matrix A)
        {
            IPreConditioner preconditioner = new Diagonal();
            IIterator iterator = Iterator.CreateDefault();
            ISolver solver = new GPBiCG(preconditioner, iterator);
            Vector solutionVector = null;
            try
            {
                solutionVector = solver.Solve(A, b);
            }
            catch (IterativeSolverException ise)
            {
                System.Console.WriteLine("Solver Exception - " + ise.Message);
                return b;
            }
            return solutionVector;
        }


        /// <summary>
        /// only for diagonal matrices
        /// </summary>
        private static Matrix UglyInverse(Matrix M)
        {
            Matrix Minv = new SparseMatrix(M.Rows, M.Columns);
            double inv;
            for (int i = 0; i < M.Rows; i++)
            {
                inv = 1 / M[i, i];
                if (double.IsInfinity(inv) || double.IsNaN(inv))
                    inv = 0;

                Minv[i, i] = inv;
            }

            return Minv;
        }

        /// <summary>
        /// solve Ax = b, return x.
        /// use PCG with jacobi precond
        /// not very fast right now, likely due to operations not being in-place
        /// </summary>
        private static Vector SolveOurs(Vector b, Matrix A)
        {
            bool ours = false; //the other is a tiny bit faster
            if (!ours)
                return Solve(b, A);

            double epsilon = Math.Pow(10, -3); //stopping criteria

            //mostly from Scientific Computing [Heath] page 474 and wikipedia for stopping criteria
            Matrix M = new DiagonalMatrix(A.GetDiagonal()); //Jacobi Preconditioner - M is diagonal
            Matrix Minv = UglyInverse(M);
            Vector x = new SparseVector(b); //initial guess
            Vector r = b - A * x; //residual
            Vector s = Minv * r;

            Vector r_new, r_Minv;
            double a, B;

            int loops = 20;
            for (int k = 1; k < loops; k++)
            {
                r = b - A * x;
                if (r.SumMagnitudes() <= epsilon)
                    k = loops;

                r_Minv = r * Minv;

                a = r_Minv.DotProduct(r) / (s * A).DotProduct(s); //compute search parameter
                x = x + a * s; //update solution
                r_new = r - a * A * s; //compute new solution

                B = (r_new * Minv).DotProduct(r_new) / r_Minv.DotProduct(r);

                s = Minv * r_new + B * s; //computer new search direction

                //book keeping
                r = r_new;
            }

            if (r.SumMagnitudes() > epsilon)
                Console.Out.WriteLine("Did not converge. Residual of {0:G4}", r.SumMagnitudes());

            return x;
        }

    }

}
